/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


//import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;



public class RobotTemplate extends SimpleRobot {

    public void autonomous() {
        //Autonomous code is entered here (none during training period)
    }


    public void operatorControl() {
        
        //All teleop code is entered here
        //"Objects" are instances of a Class.  "Methods" are functions within a Class
        //"Constructors" initialize the Objects
        
        //Define the drive channels to use on the Digital Sidecar for each motor
        int drivechannelL1 = 1;
        int drivechannelL2 = 2;
        int drivechannelR1 = 3;
        int drivechannelR2 = 4;
        
        
        RobotLED testled = new RobotLED(14);
        
        SonarDriver_1 testsonar = new SonarDriver_1();
        
        testsonar.initiate();
       
        //Define the Joystick Object
        Joystick LeftJoy = new Joystick(1);
        Joystick RightJoy = new Joystick(2);
        
        //Initialize leftY and rightY;
        double leftY = 0;
        double rightY = 0;
        
        //Call Constructors within Talon and Jaguar classes
        Talon LeftDrive1 = new Talon(drivechannelL1);
        Talon LeftDrive2 = new Talon(drivechannelL2);
        Jaguar RightDrive1 = new Jaguar(drivechannelR1);
        Jaguar RightDrive2 = new Jaguar(drivechannelR2);
        
        //Sonar
        //SonarSensor driveSonar = new SonarSensor(5,4);
        //driveSonar.initSonar();
        double sonardist=0;
        double sonarconv=0;
        
        Servo sonarservo = new Servo(5);

        //Accelerometer
        ADXL345_I2C accel = new ADXL345_I2C(1,ADXL345_I2C.DataFormat_Range.k16G);

        //Start an infinite loop
        while(true){
            //Define the input value of the Joystick Y-axis
            leftY = LeftJoy.getY();
            rightY = RightJoy.getY();
            
            
            
            //Set the Left Drive and Right Drive to Y-axis
            LeftDrive1.set(leftY);
            LeftDrive2.set(leftY);
            
            //Motors move in the opposite direction on the right side
            RightDrive1.set(-1*rightY);
            RightDrive2.set(-1*rightY);
            
            //Run servo based on sonar
            sonardist = driveSonar.getDistance();
            if(sonardist>30)
            {
                sonarconv=.52;
            }
            else
            {
                sonarconv=sonardist/30;
            }
            sonarservo.set(sonarconv);    
            //SmartDashboard.putNumber("Sonar", frontSonar.getDistance());
         
            SmartDashboard.putNumber("Sonar Inches", sonardist);
            
            
            SmartDashboard.putNumber("Accelerometer X", accel.getAcceleration(ADXL345_I2C.Axes.kX));
            SmartDashboard.putNumber("Accelerometer Z", accel.getAcceleration(ADXL345_I2C.Axes.kZ));
            SmartDashboard.putNumber("Accelerometer Y", accel.getAcceleration(ADXL345_I2C.Axes.kY));
            
            //DS updates at 20Hz, so we can wait for the next update
            Timer.delay(.05);
            testsonar.display();
           }
        
    }
    

    public void test() {
    
    }
}
